
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_l1_control.c
  * @author     baiyang
  * @date       2023-7-13
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_l1_control.h"
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static int32_t l1_bearing_error_cd(navctrl_const_t nav_ctrl);
/*----------------------------------variable----------------------------------*/
struct navigation_controller_ops l1_ops = {.nav_roll_cd                 = NULL,
                                           .lateral_acceleration        = NULL,
                                           .nav_bearing_cd              = NULL,
                                           .bearing_error_cd            = l1_bearing_error_cd,
                                           .target_bearing_cd           = NULL,
                                           .crosstrack_error_integrator = NULL,
                                           .turn_distance               = NULL,
                                           .turn_distance2              = NULL,
                                           .loiter_radius               = NULL,
                                           .update_waypoint             = NULL,
                                           .update_loiter               = NULL,
                                           .update_heading_hold         = NULL,
                                           .update_level_flight         = NULL,
                                           .reached_loiter_target       = NULL,
                                           .set_data_is_stale           = NULL,
                                           .data_is_stale               = NULL,
                                           .set_reverse                 = NULL};
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void l1_control_ctor(mb_l1_control_t l1_ctrl)
{
    l1_ctrl->navctrl.ops = &l1_ops;

    l1_ctrl->_data_is_stale = true;
    l1_ctrl->_reverse       = false;
}

static int32_t l1_bearing_error_cd(navctrl_const_t nav_ctrl)
{
    return RadiansToCentiDegrees(((mb_l1_control_t)nav_ctrl)->_bearing_error);
}
/*------------------------------------test------------------------------------*/


